//
// Created by Wirano on 2020/11/9.
//

#include "Ano_LocCtrl.h"
#include "Ano_Imu.h"
#include "Ano_FlightCtrl.h"
#include "Ano_OF.h"
#include "Ano_Parameter.h"
#include "Nizhou_drv_T265.h"

#define DX_V (T265_flight_dir.VX)
#define DY_V (T265_flight_dir.VY)


//位置速度环控制参数
_PID_arg_st loc_arg_1[2] ;

//位置速度环控制数据
_PID_val_st loc_val_1[2] ;

/*//位置速度环修正控制参数
_PID_arg_st loc_arg_1_fix[2] ;

//位置速度环修正控制数据
_PID_val_st loc_val_1_fix[2] ;*/

static uint8_t mode_f[2];//用于模式的切换

/*位置速度环PID参数初始化*/
void Loc_1level_PID_Init()
{
    if(mode_f[1] == 1)//T265PID
    {
        //normal
        loc_arg_1[X].kp = Ano_Parame.set.pid_loc_1level[KP];//0.22f  ;
        loc_arg_1[X].ki = 0.0f  ;
        loc_arg_1[X].kd_ex = 0.00f ;
        loc_arg_1[X].kd_fb = Ano_Parame.set.pid_loc_1level[KD];
        loc_arg_1[X].k_ff = 0.02f;

        loc_arg_1[Y] = loc_arg_1[X];

    }
    else
    {
        //null	
    }


}

_loc_ctrl_st loc_ctrl_1;
static float fb_speed_fix[2];
float vel_fb_d_lpf[2];
float vel_fb_h[2],vel_fb_w[2];
float vel_fb_fix_w[2];
/*位置速度环*/
void Loc_1level_Ctrl(uint16_t dT_ms,int16_t *CH_N)
{
    //TODO:记得把标志位都整理一下
    //此处需要添加T265的位置控制代码
    if(switchs.t265_on)
    {
        mode_f[1] = 1;//mode_f [1]为期望状态，mode_f[0]为目前的状态
        if(mode_f[1] != mode_f[0])//若期望的模式不同于现在的模式则需要对参数进行改变
        {
            Loc_1level_PID_Init();//loc_1为速度环
            mode_f[0] = mode_f[1];
        }
        loc_ctrl_1.exp[X] = fs.speed_set_h[X];//期望速度值赋值
        loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];

        LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);//对航线加速度进行低通滤波得到vel_fb_d_lpf
        LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);

        if(sens_hd_check.t265_ok)
        {
            loc_ctrl_1.fb[X] = DX_V + 0.03f *vel_fb_d_lpf[X];
            loc_ctrl_1.fb[Y] = DY_V + 0.03f *vel_fb_d_lpf[Y];
        }

        for(uint8_t i =0;i<2;i++)
        {
            //这一层只有PD运算
            PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）
                           loc_ctrl_1.exp[i] ,				//前馈值
                           loc_ctrl_1.exp[i] ,				//期望值（设定值）
                           loc_ctrl_1.fb[i] ,			//反馈值（）
                           &loc_arg_1[i], //PID参数结构体
                           &loc_val_1[i],	//PID数据结构体
                           50,//积分误差限幅
                           10 *flag.taking_off			//integration limit，积分限幅
            )	;

            loc_ctrl_1.out[i] = loc_val_1[i].out;	//(PD)+(I)
        }
    }//姿态模式，直接用期望速度转为角度（期望角度）
    else
    {
        mode_f[1] = 255;
        if(mode_f[1] != mode_f[0])
        {
            Loc_1level_PID_Init();
            mode_f[0] = mode_f[1];
        }
        loc_ctrl_1.out[X] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[X] ;
        loc_ctrl_1.out[Y] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[Y] ;
    }

}

_loc_ctrl_st loc_ctrl_2;


